#include <rclcpp/rclcpp.hpp>
#include <nav2_msgs/action/navigate_through_poses.hpp>
#include "rclcpp_action/rclcpp_action.hpp"

using NavigateThroughPoses = nav2_msgs::action::NavigateThroughPoses;
using GoalHandle_NavigateThroughPoses = rclcpp_action::ClientGoalHandle<NavigateThroughPoses>;

class NavigateThroughPosesClient : public rclcpp::Node
{
public:
    NavigateThroughPosesClient() : Node("navigate_through_poses_client")
    {
        action_client_ = rclcpp_action::create_client<NavigateThroughPoses>(this, "navigate_through_poses");
    }

    void send_goal()
    {
        if (!action_client_->wait_for_action_server(std::chrono::seconds(10))) {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            rclcpp::shutdown();
            return;
        }

        auto goal_msg = NavigateThroughPoses::Goal();
        goal_msg.poses.push_back(create_pose(0.0, 0.0, 1.0));
        goal_msg.poses.push_back(create_pose(-3.44, 2.50, 1.0));
        goal_msg.poses.push_back(create_pose(4.21, 2.63, 1.0));

        auto send_goal_options = rclcpp_action::Client<NavigateThroughPoses>::SendGoalOptions();
        send_goal_options.goal_response_callback = std::bind(&NavigateThroughPosesClient::goal_response_callback, this, std::placeholders::_1);
        send_goal_options.feedback_callback = std::bind(&NavigateThroughPosesClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback = std::bind(&NavigateThroughPosesClient::result_callback, this, std::placeholders::_1);

        action_client_->async_send_goal(goal_msg, send_goal_options);
    }

private:
    rclcpp_action::Client<NavigateThroughPoses>::SharedPtr action_client_;

    geometry_msgs::msg::PoseStamped create_pose(double x, double y, double w)
    {
        geometry_msgs::msg::PoseStamped pose;
        pose.header.frame_id = "map";
        pose.header.stamp = this->get_clock()->now();
        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.orientation.w = w;
        return pose;
    }

    void goal_response_callback(GoalHandle_NavigateThroughPoses::SharedPtr goal_handle)
    {
        if (!goal_handle) {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        } else {
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
    }

    void feedback_callback(GoalHandle_NavigateThroughPoses::SharedPtr, const std::shared_ptr<const NavigateThroughPoses::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Current pose index: %.3fm", feedback->distance_remaining);
    }

    void result_callback(const GoalHandle_NavigateThroughPoses::WrappedResult &result)
    {
        switch (result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "Goal succeeded!");
                break;
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
                break;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_INFO(this->get_logger(), "Goal canceled");
                break;
            default:
                RCLCPP_ERROR(this->get_logger(), "Unknown result code");
                break;
        }
        rclcpp::shutdown();
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<NavigateThroughPosesClient>();
    node->send_goal();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}